#pragma config(Sensor, in1,    sensorLeft,     sensorLineFollower)
#pragma config(Sensor, in2,    sensorMiddle,   sensorLineFollower)
#pragma config(Sensor, in3,    sensorRight,    sensorLineFollower)
#pragma config(Sensor, in4,    clawAngle,      sensorPotentiometer)
#pragma config(Sensor, in5,    armAngle,       sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightEncoder,   sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  leftEncoder,    sensorQuadEncoder)
#pragma config(Sensor, dgtl5,  sonarInput,     sensorSONAR_inch)
#pragma config(Motor,  port1,           rightMotor,    tmotorVex393, openLoop)
#pragma config(Motor,  port5,           arm,           tmotorVex393, openLoop)
#pragma config(Motor,  port6,           claw,          tmotorVex393, openLoop)
#pragma config(Motor,  port10,          leftMotor,     tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

//#include "movement.h"



//+++++++++++++++++++++++++++++++++++++++++++++| Public Integers |++++++++++++++++++++++++++++++++++++++++

int x = 0;
int y = 0;
int z = 0;
int sensorLeftInt = 0;
int sensorRightInt = 0;
int sensorMiddleInt = 0;

//+++++++++++++++++++++++++++++++++++++++++++++| Stop |++++++++++++++++++++++++++++++++++++++++++++++

void pause(int seconds) {
	motor[leftMotor] = 0;
	motor[rightMotor] = 0;
	wait1Msec(seconds * 1000);
}

//+++++++++++++++++++++++++++++++++++++++++++++| Straight Line |++++++++++++++++++++++++++++++++++++++++

void straightLine(int distance) {
	while(x < distance || y < distance) {

		x = SensorValue[rightEncoder];
		y = SensorValue[leftEncoder];

		motor[leftMotor] = 100;
		motor[rightMotor] = 100;

		if(x > y){
			motor[leftMotor] = 127;
			motor[rightMotor] = 50;
		}

		if(y > x){
			motor[leftMotor] = 50;
			motor[rightMotor] = 127;
		}

	}

	x = 0;
	y = 0;
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

}

void straightLine() {

	x = SensorValue[rightEncoder];
	y = SensorValue[leftEncoder];

	motor[leftMotor] = 100;
	motor[rightMotor] = 100;

	if(x > y){
		motor[leftMotor] = 127;
		motor[rightMotor] = 50;
	}

	if(y > x){
		motor[leftMotor] = 50;
		motor[rightMotor] = 127;
	}



	x = 0;
	y = 0;
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

}

//+++++++++++++++++++++++++++++++++++++++++++++| Right Angle Right Turn |+++++++++++++++++++++++++++++++++

void rightAngleTurnRight() {
	while(SensorValue[sensorRight] < 2000) {

		motor[leftMotor] = 90;
		motor[rightMotor] = -90;

		x = SensorValue[rightEncoder];
		y = SensorValue[leftEncoder];


	}

	x = 0;
	y = 0;
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

}

//+++++++++++++++++++++++++++++++++++++++++++++| Right Angle Left Turn |+++++++++++++++++++++++++++++++++

void rightAngleTurnLeft() {
	while(x > -420 && y < 420) {

		motor[leftMotor] = -90;
		motor[rightMotor] = 90;

		x = SensorValue[rightEncoder];
		y = SensorValue[leftEncoder];

	}

	x = 0;
	y = 0;
	SensorValue[rightEncoder] = 0;
	SensorValue[leftEncoder] = 0;

}

//+++++++++++++++++++++++++++++++++++++++++++++| Follow Line Left |+++++++++++++++++++++++++++++++++

void followLineUntilLeft() {

	for(int i = 0; i > -1; i++) {

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 100;
			motor[rightMotor] = 100;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 110;
			motor[rightMotor] = 70;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 70;
			motor[rightMotor] = 110;
		}

		if(sensorLeftInt < 1500 && sensorRightInt < 1500 && sensorMiddleInt < 1500){
			motor[leftMotor] = motor[rightMotor] = 0;
		}

		if(sensorMiddleInt > 2000 && sensorLeftInt > 2000){
			motor[leftMotor] = motor[rightMotor] = 0;
			i = ( i - i ) - 2;
		}

	}

}

//+++++++++++++++++++++++++++++++++++++++++++++| Follow Line Right |+++++++++++++++++++++++++++++++++

void followLineUntilRight() {

	for(int i = 0; i > -1; i++){

		sensorLeftInt = SensorValue[sensorLeft];
		sensorRightInt = SensorValue[sensorRight];
		sensorMiddleInt = SensorValue[sensorMiddle];

		if(sensorMiddleInt > 2000){
			motor[leftMotor] = 100;
			motor[rightMotor] = 100;
		}

		if(sensorRightInt > 2000){
			motor[leftMotor] = 110;
			motor[rightMotor] = 70;
		}

		if(sensorLeftInt > 2000){
			motor[leftMotor] = 70;
			motor[rightMotor] = 110;
		}

		if(sensorLeftInt < 1500 && sensorRightInt < 1500 && sensorMiddleInt < 1500){
			motor[leftMotor] = motor[rightMotor] = 0;
		}

		if(sensorMiddleInt > 2000 && sensorRightInt > 2000){
			motor[leftMotor] = motor[rightMotor] = 0;
			i = ( i - i ) - 2;
		}

	}

}



//+++++++++++++++++++++++++++++++++++++++++++++| MAIN |++++++++++++++++++++++++++++++++++++++++++++++++++

task main ()
{
	followLineUntilRight();
	pause(1);
	rightAngleTurnRight();

}
//++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
